#!/usr/bin/env python3

import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import PoseStamped,PointStamped

class OdomToPoseConverter:
    def __init__(self):
        # Initialize the node
        rospy.init_node('odom_to_pose_converter', anonymous=True)

        # Create a subscriber to the /odom topic
        self.odom_sub = rospy.Subscriber("/odom", Odometry, self.odom_callback)

        # Create a publisher to the /bebop2/mpc/point_of_interest topic
        self.point_pub = rospy.Publisher("/bebop2/mpc/point_of_interest", PointStamped, queue_size=10)
        self.pose_pub = rospy.Publisher("/vrpn_client_node/turbot/pose", PoseStamped, queue_size=10)

        # Initialize a PoseStamped message
        self.pose_stamped = PoseStamped()

    def odom_callback(self, odom_msg):
        # Convert Odometry message to PoseStamped
        odom=Odometry()
        odom=odom_msg
        
        point_stamped=PointStamped()
        point_stamped.header.frame_id="world"
        point_stamped.point.x=odom.pose.pose.position.x
        point_stamped.point.y=odom.pose.pose.position.y
        point_stamped.point.z=odom.pose.pose.position.z
        self.point_pub.publish(point_stamped)

        pose_stamped=PoseStamped()
        pose_stamped.header.frame_id='map'
        pose_stamped.pose=odom.pose.pose
        self.pose_pub.publish(pose_stamped)

if __name__ == '__main__':
    try:
        converter = OdomToPoseConverter()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass
